import rclpy
from rclpy.node import Node
import serial
from topic_msg.msg import CatHearing
from topic_msg.msg import CatStatus
from topic_msg.msg import CatException
from topic_msg.msg import CatSpeak

import threading
import time
import random


class CatEarsNode(Node):
    COMMON_ANOUNCE = ":0."
    EMERGENCY_ANOUNCE = ":1."
    START_ANOUNCE = ":2."
    STOP_ANOUNCE = ":3."
    HEARED = 1
    HEARING = 0

    def __init__(self, name):
        super().__init__(name)
        self._uart_send_lock = threading.Lock()
        self._anouce_status_lock = threading.Lock()
        self._hear_voice_status_lock = threading.Lock()
        self._hear_voice_status = CatEarsNode.HEARING
        self._ser = serial.Serial("/dev/ttyAMA0", 115200, timeout=1)
        self._hearing_publisher = self.create_publisher(CatHearing, "cat_hearing", 10)
        self._cat_status_subscriper = self.create_subscription(
            CatStatus, "cat_status", self._rcv_cat_status_callback, 10
        )
        self._cat_exption_subscriper = self.create_subscription(
            CatException, "cat_exception", self._rcv_cat_exption_callback, 10
        )
        self._cat_speak_subscriper = self.create_subscription(
            CatSpeak, "cat_speak", self._rcv_cat_speak_callback, 10
        )
        self._thd_uart_recv = threading.Thread(target=self._run_uart_rcv)
        self._thd_anounce = threading.Thread(target=self._run_anouce)
        self._thd_uart_recv.start()
        self._thd_anounce.start()

    def __del__(self):
        self._thd_uart_recv.join()
        self._thd_anounce.join()
        self._hearing_publisher.destroy()
        self._cat_status_subscriper.destroy()
        self._ser.close()

    def _rcv_cat_speak_callback(self, msg):
        if msg.announce == CatSpeak.USUAL:
            self._invoke_anounce(CatEarsNode.COMMON_ANOUNCE)
        elif msg.announce == CatSpeak.ERGENCY:
            self._invoke_anounce(CatEarsNode.EMERGENCY_ANOUNCE)
        else:
            pass

    def _rcv_cat_exption_callback(self, msg):
        self._invoke_anounce(CatEarsNode.EMERGENCY_ANOUNCE)

    def _rcv_cat_status_callback(self, msg):
        self._anouce_status = msg.status
        if (
            self._anouce_status == CatStatus.CAT_DONT_ANOUNCE
            or self._anouce_status == CatStatus.CAT_STOP
        ):
            # self.get_logger().info(f"cann't anounce")
            self._invoke_anounce(CatEarsNode.STOP_ANOUNCE)
        elif (
            self._anouce_status == CatStatus.CAT_ACTIVE
            or self._anouce_status == CatStatus.CAT_DONT_MOVE
        ):
            # self.get_logger().info(f"can anounce")
            self._invoke_anounce(CatEarsNode.START_ANOUNCE)
            # self._invoke_anounce(CatEarsNode.COMMON_ANOUNCE)

    def _run_anouce(self):
        while rclpy.ok():
            time.sleep(4)
            with self._hear_voice_status_lock:
                if self._hear_voice_status == CatEarsNode.HEARED:
                    self._invoke_anounce(CatEarsNode.COMMON_ANOUNCE)
                    self._hear_voice_status = CatEarsNode.HEARING
                else:
                    if random.randint(0, 400) % 10 == 0:
                        self._invoke_anounce(CatEarsNode.COMMON_ANOUNCE)
                    else:
                        pass

    def _run_uart_rcv(self):
        while rclpy.ok():
            count = self._ser.inWaiting()  # 获取串口缓冲区数据
            if count != 0:
                recv = self._ser.read(self._ser.in_waiting).decode("utf-8")
                if len(recv) != 5:
                    # self.get_logger().info(f"recv cmd: {recv}")
                    time.sleep(0.1)  # 延时0.1秒，免得CPU出问题
                    continue
                else:
                    if recv[0] != ":" or recv[4] != ".":
                        # self.get_logger().info(f"recv form err: {recv}")
                        time.sleep(0.1)  # 延时0.1秒，免得CPU出问题
                        continue
                    else:
                        cmd = int(recv[1:4])
                        # self.get_logger().info(f"recv cmd {recv} to int: {cmd}")
                        self._deal_command(cmd)
            else:
                pass
            time.sleep(0.1)  # 延时0.1秒，免得CPU出问题

    def _deal_command(self, cmd):
        if (
            cmd < CatHearing.AWAKE_1
            or (cmd > CatHearing.AWAKE_5 and cmd < CatHearing.DONT_MOVE_1)
            or (cmd > CatHearing.SHAKE_HEAD and cmd < CatHearing.HAPPY)
            or cmd > CatHearing.HELLO_3
        ):
            # self.get_logger().info(f"deal cmd over range: {cmd}")
            self._invoke_anounce(CatEarsNode.EMERGENCY_ANOUNCE)
        else:
            # self.get_logger().info(f"deal cmd push cmd: {cmd}")
            msg = CatHearing()
            msg.cmd = cmd
            self._hearing_publisher.publish(msg)
            if cmd >= CatHearing.AWAKE_1 and cmd <= CatHearing.AWAKE_5:
                with self._hear_voice_status_lock:
                    self._hear_voice_status = CatEarsNode.HEARED

    def _invoke_anounce(self, anounce):
        with self._uart_send_lock:
            self._ser.write(anounce.encode())
            time.sleep(0.1)  # 延时0.1秒，免得CPU出问题


def main():
    rclpy.init()
    node = CatEarsNode("cat_ears")
    rclpy.spin(node)
    rclpy.shutdown()
